#include "geometry_msgs/msg/transform_stamped.hpp"
#include "memory"
#include "rclcpp/rclcpp.hpp"
#include "robot_interfaces/msg/robot_state.hpp"
#include "string"
#include "tf2/utils.h"
#include "tf2_ros/transform_broadcaster.h"
#include <fstream>
#include <string>
#include <tf2/convert.h>
#include <vector>
int count1 = 0;
int count2 = 0;
class RobotSensor : public rclcpp::Node {
public:
  RobotSensor(const std::string &node_name) : rclcpp::Node(node_name) {
    this->declare_parameter<float>("publish_rate", 100.0);
    this->declare_parameter<std::string>(
        "file_path",
        "/home/ccy/ros_ws/chapt1/robot_ws/src/robot_sensor/data1.txt");
    this->declare_parameter<std::string>("observation_frame", "base");
    float publish_rate;
    this->get_parameter("publish_rate", publish_rate);
    int timer_period = static_cast<int>((1.0 / publish_rate) * 1000);
    std::fstream file;
    std::string file_path_;
    this->get_parameter("file_path", file_path_);
    file.open("file_path",
              std::ios::in);
    if (file.is_open()) {
      std::string line;
      while (std::getline(file, line)) {
        std::string sub_line1 = line.substr(0, line.find("  "));
        std::string sub_line2 = line.substr(
            line.find("  ") + 2, line.find("  ", line.find("  ") + 1));
        std::string sub_line3 = line.substr(
            line.find("  ", line.find("  ") + 1) + 2,
            line.find("  ", line.find("  ", line.find("  ") + 1) + 1));
        std::string sub_line4 = line.substr(
            line.find("  ", line.find("  ", line.find("  ") + 1) + 1) + 2);

        target_base_x.push_back(std::stod(sub_line1));
        target_base_y.push_back(std::stod(sub_line2));
        base_world_x.push_back(std::stod(sub_line3));
        base_world_y.push_back(std::stod(sub_line4));
      }
    } else {
      RCLCPP_ERROR(this->get_logger(), "Could not open file");
    }
    file.close();
    std::string file_path = this->get_parameter("file_path").as_string();
    RCLCPP_INFO(this->get_logger(), "Hello, I am a robot sensor!");

    data_publisher_ =
        this->create_publisher<robot_interfaces::msg::RobotState>("data", 10);

    RCLCPP_INFO(this->get_logger(), "Publishing rate: %d", timer_period);
    timer_ =
        this->create_wall_timer(std::chrono::milliseconds(timer_period),
                                std::bind(&RobotSensor::timer_callback, this));

    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
  }


private:
  void publiTransforms() {
    std::string observation_frame_;
    this->get_parameter("observation_frame", observation_frame_);
    geometry_msgs::msg::TransformStamped target_to_base;
    target_to_base.header.stamp = this->now();
    target_to_base.header.frame_id = observation_frame_;
    target_to_base.child_frame_id = "target";

    target_to_base.transform.translation.x = target_base_x[count1];
    target_to_base.transform.translation.y = target_base_y[count1];
    // target_to_base.transform.translation.z = 0.0;
    tf2::Quaternion quat_target_to_base;
    quat_target_to_base.setRPY(0.0, 0.0, 0.0);
    target_to_base.transform.rotation.x = quat_target_to_base.x();
    target_to_base.transform.rotation.y = quat_target_to_base.y();
    // target_to_base.transform.rotation.z = quat_target_to_base.z();
    target_to_base.transform.rotation.w = quat_target_to_base.w();

    geometry_msgs::msg::TransformStamped base_world;
    base_world.header.stamp = this->now();
    base_world.header.frame_id = "world";
    base_world.child_frame_id = observation_frame_;
    base_world.transform.translation.x = base_world_x[count1];
    base_world.transform.translation.y = base_world_y[count1];
    // base_world.transform.translation.z = 0.0;
    tf2::Quaternion quat_base_world;
    quat_base_world.setRPY(0.0, 0.0, 0.0);
    base_world.transform.rotation.x = quat_base_world.x();
    base_world.transform.rotation.y = quat_base_world.y();
    // base_world.transform.rotation.z = quat_base_world.z();
    base_world.transform.rotation.w = quat_base_world.w();

    tf_broadcaster_->sendTransform(target_to_base);
    tf_broadcaster_->sendTransform(base_world);
    count1++;
    if (count1 >= static_cast<int>(base_world_x.size())) {
      RCLCPP_INFO(
          this->get_logger(),
          "Published the required number of transforms. Shutting down.");
      rclcpp::shutdown();
      return;
    }
    // rate.sleep();
  }
  void timer_callback() {
    publiTransforms();
    robot_interfaces::msg::RobotState msg;
    // RCLCPP_INFO(this->get_logger(), "Publishing data");
    msg.target_base_x = target_base_x[count2];
    msg.target_base_y = target_base_y[count2];
    msg.base_world_x = base_world_x[count2];
    msg.base_world_y = base_world_y[count2];
    // RCLCPP_INFO(this->get_logger(), "target_base_x: %f", msg.target_base_x);
    data_publisher_->publish(msg);
    count2++;
  }

  std::vector<float> target_base_x;
  std::vector<float> target_base_y;
  std::vector<float> base_world_x;
  std::vector<float> base_world_y;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  rclcpp::Publisher<robot_interfaces::msg::RobotState>::SharedPtr
      data_publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
};
int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<RobotSensor>("robot_sensor");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
